import numpy as np
import threading
import time
from enum import IntEnum
from utils.logger import run_time

# 移除unitree_sdk2py相关的导入，使用模拟数据

FangnuoNumMotors = 13  # 0-12，包括base_link和双臂

class MotorState:
    def __init__(self):
        self.q = None
        self.dq = None

class FangnuoLowState:
    def __init__(self):
        self.motor_state = [MotorState() for _ in range(FangnuoNumMotors)]

class DataBuffer:
    def __init__(self):
        self.data = None
        self.lock = threading.Lock()

    def GetData(self):
        with self.lock:
            return self.data

    def SetData(self, data):
        with self.lock:
            self.data = data


class FangnuoJointIndex(IntEnum):
    # left arm
    kLeftShoulderYaw = 1
    kLeftShoulderPitch = 2
    kLeftElbow = 3
    kLeftWristRoll = 4
    kLeftWristPitch = 5
    kLeftWristEnd = 6
    # right arm
    kRightShoulderYaw = 7
    kRightShoulderPitch = 8
    kRightElbow = 9
    kRightWristRoll = 10
    kRightWristPitch = 11
    kRightWristEnd = 12

class FangnuoIndex(IntEnum):
    # base_link
    base_link = 0
    # left arm
    kLeftShoulderYaw = 1
    kLeftShoulderPitch = 2
    kLeftElbow = 3
    kLeftWristRoll = 4
    kLeftWristPitch = 5
    kLeftWristEnd = 6
    # right arm
    kRightShoulderYaw = 7
    kRightShoulderPitch = 8
    kRightElbow = 9
    kRightWristRoll = 10
    kRightWristPitch = 11
    kRightWristEnd = 12

class FangnuoCarController:
    def __init__(self):
        print("Initialize FangnuoCarController (模拟版本)...")
        # 初始化目标关节角度和力矩，12个关节（不包括base_link）
        self.q_target = np.zeros(12)
        self.tauff_target = np.zeros(12)

        # 设置PID控制参数
        self.kp_high = 300.0
        self.kd_high = 3.0
        self.kp_low = 80.0
        self.kd_low = 3.0
        self.kp_wrist = 40.0
        self.kd_wrist = 1.5

        # 创建模拟的电机状态数据
        self.all_motor_q = np.zeros(FangnuoNumMotors)
        self.all_motor_dq = np.zeros(FangnuoNumMotors)
        self.arm_velocity_limit = 20.0
        self.control_dt = 1.0 / 250.0

        self._speed_gradual_max = False
        self._gradual_start_time = None
        self._gradual_time = None

        # 创建模拟的数据缓冲区
        self.lowstate_buffer = DataBuffer()
        lowstate = FangnuoLowState()
        for id in range(FangnuoNumMotors):
            lowstate.motor_state[id].q = 0.0
            lowstate.motor_state[id].dq = 0.0
        self.lowstate_buffer.SetData(lowstate)

        # 初始化控制线程
        self.ctrl_lock = threading.Lock()
        self.publish_thread = threading.Thread(target=self._ctrl_motor_state)
        self.publish_thread.daemon = True
        self.publish_thread.start()

        print(f"Current all body motor state q:\n{self.all_motor_q} \n")
        print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n")
        print("Initialize FangnuoCarController OK!\n")

    def _update_motor_state(self):
        """更新模拟的电机状态"""
        # 在实际实现中，这个函数会从硬件读取数据
        # 在模拟版本中，我们只是简单地更新内部状态
        lowstate = FangnuoLowState()
        
        # 逐渐将当前状态向目标状态靠近，模拟电机运动
        with self.ctrl_lock:
            target_q = self.q_target
        
        current_q = self.get_current_dual_arm_q()
        delta = target_q - current_q
        motion_scale = np.max(np.abs(delta)) / (self.arm_velocity_limit * self.control_dt)
        new_q = current_q + delta / max(motion_scale, 1.0)
        
        # 更新模拟的电机状态
        idx = 0
        for id in FangnuoJointIndex:
            self.all_motor_q[id.value] = new_q[idx]
            idx += 1
            
        # 更新数据缓冲区
        for id in range(FangnuoNumMotors):
            lowstate.motor_state[id].q = self.all_motor_q[id]
            lowstate.motor_state[id].dq = 0.0  # 简化处理，不模拟速度
        
        self.lowstate_buffer.SetData(lowstate)

    def clip_arm_q_target(self, target_q, velocity_limit):
        """限制关节运动速度，确保平滑过渡"""
        current_q = self.get_current_dual_arm_q()
        delta = target_q - current_q
        motion_scale = np.max(np.abs(delta)) / (velocity_limit * self.control_dt)
        cliped_arm_q_target = current_q + delta / max(motion_scale, 1.0)
        return cliped_arm_q_target

    def _ctrl_motor_state(self):
        """控制电机状态的线程函数（模拟版本）"""
        while True:
            start_time = time.time()
            
            # 更新模拟的电机状态
            self._update_motor_state()
            
            # 如果启用了渐进式速度增加
            if self._speed_gradual_max is True:
                t_elapsed = start_time - self._gradual_start_time
                self.arm_velocity_limit = 20.0 + (10.0 * min(1.0, t_elapsed / self._gradual_time))
            
            current_time = time.time()
            all_t_elapsed = current_time - start_time
            sleep_time = max(0, (self.control_dt - all_t_elapsed))
            time.sleep(sleep_time)

    def ctrl_dual_arm(self, q_target, tauff_target):
        '''设置左右臂的目标关节角度和力矩'''
        with self.ctrl_lock:
            self.q_target = q_target
            self.tauff_target = tauff_target

    def get_mode_machine(self):
        '''返回当前模式（模拟版本）'''
        return 0  # 返回模拟的模式值
    
    def get_current_motor_q(self):
        '''返回所有电机的当前关节角度（模拟版本）'''
        if self.lowstate_buffer.GetData() is None:
            return np.zeros(FangnuoNumMotors)
        return np.array([self.lowstate_buffer.GetData().motor_state[id.value].q for id in FangnuoIndex])
    
    def get_current_dual_arm_q(self):
        '''返回左右臂电机的当前关节角度（模拟版本）'''
        if self.lowstate_buffer.GetData() is None:
            return np.zeros(12)
        return np.array([self.lowstate_buffer.GetData().motor_state[id.value].q for id in FangnuoJointIndex])
    
    def get_current_dual_arm_dq(self):
        '''返回左右臂电机的当前关节速度（模拟版本）'''
        if self.lowstate_buffer.GetData() is None:
            return np.zeros(12)
        return np.array([self.lowstate_buffer.GetData().motor_state[id.value].dq for id in FangnuoJointIndex])
    
    def ctrl_dual_arm_go_home(self):
        '''将左右臂移动到初始位置（关节角度为零）'''
        print("[FangnuoCarController] ctrl_dual_arm_go_home start...")
        with self.ctrl_lock:
            self.q_target = np.zeros(12)
            # self.tauff_target = np.zeros(12)
        tolerance = 0.05  # 判断关节角度是否接近零的容差阈值
        while True:
            current_q = self.get_current_dual_arm_q()
            if np.all(np.abs(current_q) < tolerance):
                print("[FangnuoCarController] both arms have reached the home position.")
                break
            time.sleep(0.05)

    def speed_gradual_max(self, t=5.0):
        '''设置臂的速度逐渐增加到最大值，参数t是所需的总时间（秒）'''
        self._gradual_start_time = time.time()
        self._gradual_time = t
        self._speed_gradual_max = True

    def speed_instant_max(self):
        '''立即将臂的速度设置为最大值'''
        self.arm_velocity_limit = 30.0

    def _Is_wrist_motor(self, motor_index):
        '''判断是否为手腕电机'''
        wrist_motors = [
            FangnuoIndex.kLeftWristRoll.value,
            FangnuoIndex.kLeftWristPitch.value,
            FangnuoIndex.kLeftWristEnd.value,
            FangnuoIndex.kRightWristRoll.value,
            FangnuoIndex.kRightWristPitch.value,
            FangnuoIndex.kRightWristEnd.value,
        ]
        return motor_index.value in wrist_motors


# 夹爪相关的类定义
class Gripper_JointIndex(IntEnum):
    kLeftGripper = 1
    kRightGripper = 0


class FangnuoCar:
    def __init__(self, use_real_arms=False):
        """
        FangnuoCar对象封装，用于控制左臂和右臂
        
        参数:
        use_real_arms: 是否使用真实的机械臂，如果为True则初始化piper控制器，否则使用模拟控制器
        """
        print("初始化 FangnuoCar 对象...")
        
        # 初始化左臂和右臂控制器
        self.use_real_arms = use_real_arms
        self.left_arm = None
        self.right_arm = None
        
        # 初始化夹爪位置变量
        self.left_gripper_position = 0.0  # 左夹爪位置，单位为mm
        self.right_gripper_position = 0.0  # 右夹爪位置，单位为mm
        
        if self.use_real_arms:
            try:
                # 导入piper_sdk
                from piper_sdk import C_PiperInterface_V2
                
                print("初始化真实机械臂...")
                
                # 1. 初始化左臂
                print("初始化左臂...")
                self.left_arm = C_PiperInterface_V2(can_name="left")
                self.left_arm.ConnectPort()
                
                # 重置左臂
                self.left_arm.MotionCtrl_1(0x02, 0, 0)  # 恢复
                self.left_arm.MotionCtrl_2(0, 0, 0, 0x00)  # 位置速度模式
                
                # 使能左臂
                self.left_arm.EnableArm(7)
                self._enable_arm(self.left_arm, "左臂")
                self.left_arm.GripperCtrl(0, 1000, 0x01, 0)
                
                # 2. 初始化右臂
                print("初始化右臂...")
                self.right_arm = C_PiperInterface_V2(can_name="right")
                self.right_arm.ConnectPort()
                
                # 重置右臂
                self.right_arm.MotionCtrl_1(0x02, 0, 0)  # 恢复
                self.right_arm.MotionCtrl_2(0, 0, 0, 0x00)  # 位置速度模式
                
                # 使能右臂
                self.right_arm.EnableArm(7)
                self._enable_arm(self.right_arm, "右臂")
                self.right_arm.GripperCtrl(0, 1000, 0x01, 0)
                
                print("真实机械臂初始化完成")
                
            except ImportError:
                print("警告: 无法导入piper_sdk，切换到模拟模式")
                self.use_real_arms = False
            except Exception as e:
                print(f"错误: 初始化真实机械臂失败 - {str(e)}")
                self.use_real_arms = False
        
        # 如果不使用真实机械臂或初始化失败，使用模拟控制器
        if not self.use_real_arms:
            print("使用模拟机械臂控制器...")
            self.car_controller = FangnuoCarController()
        
        print("FangnuoCar 对象初始化完成!\n")
    
    def _enable_arm(self, piper, arm_name):
        """
        使能机械臂并检测使能状态
        
        参数:
        piper: 机械臂控制器对象
        arm_name: 机械臂名称（用于日志输出）
        """
        enable_flag = False
        timeout = 5  # 设置超时时间（秒）
        start_time = time.time()
        elapsed_time_flag = False
        
        while not enable_flag:
            elapsed_time = time.time() - start_time
            print(f"--------------------")
            
            # 检查所有电机的使能状态
            enable_flag = piper.GetArmLowSpdInfoMsgs().motor_1.foc_status.driver_enable_status and \
                piper.GetArmLowSpdInfoMsgs().motor_2.foc_status.driver_enable_status and \
                piper.GetArmLowSpdInfoMsgs().motor_3.foc_status.driver_enable_status and \
                piper.GetArmLowSpdInfoMsgs().motor_4.foc_status.driver_enable_status and \
                piper.GetArmLowSpdInfoMsgs().motor_5.foc_status.driver_enable_status and \
                piper.GetArmLowSpdInfoMsgs().motor_6.foc_status.driver_enable_status
            
            print(f"{arm_name}使能状态: {enable_flag}")
            piper.EnableArm(7)  # 使能所有电机
            piper.GripperCtrl(0, 1000, 0x01, 0)  # 控制夹爪
            print(f"--------------------")
            
            # 检查是否超过超时时间
            if elapsed_time > timeout:
                print(f"{arm_name}使能超时...")
                elapsed_time_flag = True
                enable_flag = True  # 强制退出循环
                break
                
            time.sleep(1)
        
        if elapsed_time_flag:
            print(f"{arm_name}自动使能超时，请检查硬件连接")
        else:
            print(f"{arm_name}使能成功")

    def enable_dual_arm(self):
        """
        使能左右臂
        """
        self._enable_arm(self.left_arm, "左臂")
        self._enable_arm(self.right_arm, "右臂")
    
    @run_time
    def ctrl_dual_arm(self, q_target, tauff_target=None):
        """
        设置左右臂的目标关节角度和力矩
        
        参数:
        q_target: 目标关节角度，12个关节的数组
        tauff_target: 目标力矩，如果为None则使用零力矩
        """
        if tauff_target is None:
            tauff_target = np.zeros(12)
            
        if self.use_real_arms:
            try:
                # 分离左右臂的目标角度
                left_arm_q = q_target[0:6]  # 左臂关节角度 (6个关节)
                right_arm_q = q_target[6:12]  # 右臂关节角度 (6个关节)

                #左臂第四/六关节与urdf模型求解方向不一致，需要翻转
                left_arm_q[3] = -left_arm_q[3]
                left_arm_q[5] = -left_arm_q[5]
                #右臂第四/六关节与urdf模型求解方向不一致，需要翻转
                right_arm_q[3] = -right_arm_q[3]
                right_arm_q[5] = -right_arm_q[5]

                
                # 计算因子，将弧度转换为电机内部单位
                factor = 57295.7795  # 1000*180/3.1415926
                
                # 控制左臂
                joint_0_left = round(left_arm_q[0] * factor)
                joint_1_left = round(left_arm_q[1] * factor)
                joint_2_left = round(left_arm_q[2] * factor)
                joint_3_left = round(left_arm_q[3] * factor)
                joint_4_left = round(left_arm_q[4] * factor)
                joint_5_left = round(left_arm_q[5] * factor)
                
                self.left_arm.MotionCtrl_2(0x01, 0x01, 100, 0x00)  # 设置位置速度控制模式
                self.left_arm.JointCtrl(joint_0_left, joint_1_left, joint_2_left, joint_3_left, joint_4_left, joint_5_left)
                
                # 控制右臂
                joint_0_right = round(right_arm_q[0] * factor)
                joint_1_right = round(right_arm_q[1] * factor)
                joint_2_right = round(right_arm_q[2] * factor)
                joint_3_right = round(right_arm_q[3] * factor)
                joint_4_right = round(right_arm_q[4] * factor)
                joint_5_right = round(right_arm_q[5] * factor)
                
                self.right_arm.MotionCtrl_2(0x01, 0x01, 100, 0x00)  # 设置位置速度控制模式
                self.right_arm.JointCtrl(joint_0_right, joint_1_right, joint_2_right, joint_3_right, joint_4_right, joint_5_right)
                
            except Exception as e:
                print(f"错误: 控制真实机械臂失败 - {str(e)}")
        else:
            # 使用模拟控制器
            self.car_controller.ctrl_dual_arm(q_target, tauff_target)
    @run_time
    def go_home(self):
        """
        重置左臂和右臂到初始位置
        """
        print("[FangnuoCar] 重置左臂和右臂到初始位置...")
        
        if self.use_real_arms:
            try:
                # 创建零位置目标
                zero_position = np.zeros(12)
                zero_position[4] = 0.355 # left arm 5 joint
                # zero_position[8] = 0.037 # right arm 3 joint
                zero_position[10] = 0.355 # right arm 5 joint
                # zero_position[11] = 0.475 # right arm 6 joint
                
                
                self.ctrl_dual_arm(zero_position)
                
                # 等待机械臂达到目标位置
                print("等待机械臂达到初始位置...")
                tolerance = 0.05  # 容差阈值
                max_wait_time = 10  # 最长等待时间（秒）
                start_time = time.time()
                
                while True:
                    # 获取当前状态
                    status = self.status()
                    left_arm_q = status['left_arm']['position']
                    right_arm_q = status['right_arm']['position']
                    
                    # 计算当前位置与目标位置的差异
                    left_arm_diff = np.linalg.norm(left_arm_q - zero_position[0:6])
                    right_arm_diff = np.linalg.norm(right_arm_q - zero_position[6:12])
                    
                    # 检查是否达到目标位置
                    if left_arm_diff < tolerance and right_arm_diff < tolerance:
                        print("[FangnuoCar] 左臂和右臂已达到HOME位置")
                        break
                    
                    # 检查是否超过最大等待时间
                    if time.time() - start_time > max_wait_time:
                        print("[FangnuoCar] 重置超时，机械臂可能卡住了")
                        break
                    
                    time.sleep(0.1)
                
            except Exception as e:
                print(f"错误: 重置真实机械臂失败 - {str(e)}")
        else:
            # 使用模拟控制器
            self.car_controller.go_home()
    
    def stop(self):
        """
        停止左臂和右臂的运动
        通过将目标设置为当前位置来实现停止
        """
        print("[FangnuoCar] 停止左臂和右臂运动...")
        
        if self.use_real_arms:
            try:
                # 直接使用停止命令，而不是尝试发送当前位置
                # 这样可以避免关节值超出范围的问题
                try:
                    # 左臂停止
                    self.left_arm.MotionCtrl_1(0x01, 0, 0)  # 停止命令
                    print("左臂停止命令已发送")
                except Exception as e:
                    print(f"左臂停止失败 - {str(e)}")
                
                try:
                    # 右臂停止
                    self.right_arm.MotionCtrl_1(0x01, 0, 0)  # 停止命令
                    print("右臂停止命令已发送")
                except Exception as e:
                    print(f"右臂停止失败 - {str(e)}")
                    
                # 等待停止命令生效
                time.sleep(0.5)
                    
            except Exception as e:
                print(f"错误: 停止真实机械臂失败 - {str(e)}")
        else:
            # 使用模拟控制器
            current_q = self.car_controller.get_current_dual_arm_q()
            self.car_controller.ctrl_dual_arm(current_q, np.zeros(12))
            
        print("[FangnuoCar] 左臂和右臂已停止")
    
    def reset(self):
        """
        重置左臂和右臂到初始位置
        """
        print("[FangnuoCar] 重置左臂和右臂到初始位置...")
        
        if self.use_real_arms:
            try:
                
                self.left_arm.MotionCtrl_1(0x02, 0, 0)  # 恢复
                self.left_arm.MotionCtrl_2(0, 0, 0, 0x00)  # 位置速度模式
                self.right_arm.MotionCtrl_1(0x02, 0, 0)  # 恢复
                self.right_arm.MotionCtrl_2(0, 0, 0, 0x00)  # 位置速度模式
            except Exception as e:
                print(f"错误: 重置真实机械臂失败 - {str(e)}")
        else:
            # 使用模拟控制器
            self.car_controller.ctrl_dual_arm_go_home()
            
        print("[FangnuoCar] 左臂和右臂已重置到初始位置")
    
    def _get_joint_position(self, piper):
        """
        获取机械臂所有关节的位置
        
        参数:
            piper: 机械臂接口实例
            
        返回:
            positions: 各关节位置（弧度）
        """
        # 获取关节位置
        joint_msgs = piper.GetArmJointMsgs()
        
        # 提取位置信息（单位：度）并转换为弧度
        positions = np.zeros(6)
        positions[0] = joint_msgs.joint_state.joint_1 * 0.001 * np.pi / 180.0
        positions[1] = joint_msgs.joint_state.joint_2 * 0.001 * np.pi / 180.0
        positions[2] = joint_msgs.joint_state.joint_3 * 0.001 * np.pi / 180.0
        positions[3] = joint_msgs.joint_state.joint_4 * 0.001 * np.pi / 180.0
        positions[4] = joint_msgs.joint_state.joint_5 * 0.001 * np.pi / 180.0
        positions[5] = joint_msgs.joint_state.joint_6 * 0.001 * np.pi / 180.0
        
        return positions
    
    def _calculate_joint_velocity(self, piper, prev_positions=None, interval=0.05):
        """
        通过计算两个时间点的位置差来估算关节速度
        
        参数:
            piper: 机械臂接口实例
            prev_positions: 前一时刻的位置，如果为None则实时计算
            interval: 采样时间间隔（秒）
            
        返回:
            velocities: 各关节速度（弧度/秒）
        """
        if prev_positions is None:
            # 第一次读取位置
            pos1 = self._get_joint_position(piper)
            
            # 等待一小段时间
            time.sleep(interval)
            
            # 第二次读取位置
            pos2 = self._get_joint_position(piper)
        else:
            # 使用前一时刻的位置
            pos1 = prev_positions
            pos2 = self._get_joint_position(piper)
        
        # 计算速度（弧度/秒）
        velocities = (pos2 - pos1) / interval
        
        return velocities, pos2
    
    def status(self):
        """
        获取左臂和右臂关节的位置和速度
        
        返回:
        dict: 包含左臂和右臂关节位置和速度的字典
        """
        if self.use_real_arms:
            try:
                # 创建状态字典
                status_dict = {
                    "left_arm": {
                        "position": np.zeros(6),
                        "velocity": np.zeros(6)
                    },
                    "right_arm": {
                        "position": np.zeros(6),
                        "velocity": np.zeros(6)
                    }
                }
                
                # 使用改进的方法获取左臂位置
                left_arm_position = self._get_joint_position(self.left_arm)
                
                # 计算左臂速度
                left_arm_velocity, updated_left_position = self._calculate_joint_velocity(self.left_arm, left_arm_position)
                
                # 使用改进的方法获取右臂位置
                right_arm_position = self._get_joint_position(self.right_arm)
                
                # 计算右臂速度
                right_arm_velocity, updated_right_position = self._calculate_joint_velocity(self.right_arm, right_arm_position)
                
                # 更新状态字典
                status_dict["left_arm"]["position"] = updated_left_position
                status_dict["left_arm"]["velocity"] = left_arm_velocity
                status_dict["right_arm"]["position"] = updated_right_position
                status_dict["right_arm"]["velocity"] = right_arm_velocity
                
                return status_dict
                
            except Exception as e:
                print(f"错误: 获取真实机械臂状态失败 - {str(e)}")
                # 如果获取真实机械臂状态失败，返回模拟状态
                return self._get_simulated_status()
        else:
            # 使用模拟控制器
            return self._get_simulated_status()
    
    def _get_simulated_status(self):
        """
        从模拟控制器获取状态
        """
        # 获取当前关节角度和速度
        current_q = self.car_controller.get_current_dual_arm_q()
        current_dq = self.car_controller.get_current_dual_arm_dq()
        
        # 分离左臂和右臂的数据
        left_arm_q = current_q[0:6]  # 左臂关节角度 (6个关节)
        right_arm_q = current_q[6:12]  # 右臂关节角度 (6个关节)
        left_arm_dq = current_dq[0:6]  # 左臂关节速度
        right_arm_dq = current_dq[6:12]  # 右臂关节速度
        
        # 创建状态字典
        status_dict = {
            "left_arm": {
                "position": left_arm_q,
                "velocity": left_arm_dq
            },
            "right_arm": {
                "position": right_arm_q,
                "velocity": right_arm_dq
            }
        }
        
        return status_dict
    
    def speed_gradual_max(self, t=5.0):
        """
        设置臂的速度逐渐增加到最大值
        
        参数:
        t: 所需的总时间（秒）
        """
        if not self.use_real_arms:
            self.car_controller.speed_gradual_max(t)
        # 对于真实机械臂，速度控制已经在ctrl_dual_arm中设置
    
    def speed_instant_max(self):
        """
        立即将臂的速度设置为最大值
        """
        if not self.use_real_arms:
            self.car_controller._speed_gradual_max = False
            self.car_controller.arm_velocity_limit = 30.0
            
    def control_left_gripper(self, position, force=1000):
        """
        控制左臂夹爪
        
        参数:
        position: 夹爪位置，单位为mm，0表示完全闭合，最大值取决于夹爪类型（通常为50mm）
        force: 夹爪力矩，默认为1000
        """
        self.left_gripper_position = position
        
        if self.use_real_arms and self.left_arm is not None:
            try:
                # 将位置转换为微米单位
                position_um = int(position * 1000)
                # 控制夹爪，0x01表示位置控制模式，0表示无特殊选项
                self.left_arm.GripperCtrl(position_um, force, 0x01, 0)
                # print(f"左臂夹爪控制: 位置={position}mm, 力矩={force}")
                return True
            except Exception as e:
                print(f"左臂夹爪控制失败: {e}")
                return False
        else:
            # 模拟模式下，只更新内部状态
            # print(f"模拟左臂夹爪控制: 位置={position}mm, 力矩={force}")
            return True

    def control_right_gripper(self, position, force=1000):
        """
        控制右臂夹爪
        
        参数:
        position: 夹爪位置，单位为mm，0表示完全闭合，最大值取决于夹爪类型（通常为50mm）
        force: 夹爪力矩，默认为1000
        """
        self.right_gripper_position = position
        
        if self.use_real_arms and self.right_arm is not None:
            try:
                # 将位置转换为微米单位
                position_um = int(position * 1000)
                # 控制夹爪，0x01表示位置控制模式，0表示无特殊选项
                self.right_arm.GripperCtrl(position_um, force, 0x01, 0)
                # print(f"右臂夹爪控制: 位置={position}mm, 力矩={force}")
                return True
            except Exception as e:
                print(f"右臂夹爪控制失败: {e}")
                return False
        else:
            # 模拟模式下，只更新内部状态
            # print(f"模拟右臂夹爪控制: 位置={position}mm, 力矩={force}")
            return True
    @run_time
    def control_dual_grippers(self, left_position, right_position, force=1000):
        """
        同时控制左右臂夹爪
        
        参数:
        left_position: 左夹爪位置，单位为mm
        right_position: 右夹爪位置，单位为mm
        force: 夹爪力矩，默认为1000
        """
        left_success = self.control_left_gripper(left_position, force)
        right_success = self.control_right_gripper(right_position, force)
        return left_success and right_success
    
    def get_gripper_state(self):
        """
        获取左右夹爪的当前状态
        
        返回:
        dict: 包含左右夹爪位置的字典
        """
        if self.use_real_arms:
            left_state = None
            right_state = None
            
            # 获取左臂夹爪状态
            if self.left_arm is not None:
                try:
                    left_gripper_msgs = self.left_arm.GetArmGripperMsgs()
                    # print(f"左臂夹爪状态: {left_gripper_msgs}")
                    # print(f"左臂夹爪状态: {left_gripper_msgs.gripper_state.grippers_angle}")
                    # 将微米单位转换为毫米
                    left_state = left_gripper_msgs.gripper_state.grippers_angle / 1000.0
                except Exception as e:
                    print(f"获取左臂夹爪状态失败: {e}")
                    left_state = self.left_gripper_position
            
            # 获取右臂夹爪状态
            if self.right_arm is not None:
                try:
                    right_gripper_msgs = self.right_arm.GetArmGripperMsgs()
                    # 将微米单位转换为毫米
                    right_state = right_gripper_msgs.gripper_state.grippers_angle / 1000.0
                except Exception as e:
                    print(f"获取右臂夹爪状态失败: {e}")
                    right_state = self.right_gripper_position
            
            return {
                "left_gripper": left_state if left_state is not None else self.left_gripper_position,
                "right_gripper": right_state if right_state is not None else self.right_gripper_position
            }
        else:
            # 模拟模式下，返回内部状态
            return {
                "left_gripper": self.left_gripper_position,
                "right_gripper": self.right_gripper_position
            }


class Gripper_Controller:
    def __init__(self, left_hand_array, right_hand_array, dual_gripper_data_lock=None, dual_gripper_state_out=None, dual_gripper_action_out=None, 
                 filter=True, fps=200.0, Unit_Test=False):
        """
        模拟版本的Gripper_Controller，不依赖于实际的硬件接口
        
        left_hand_array: [input] 左手骨骼数据（从XR设备获取）
        right_hand_array: [input] 右手骨骼数据（从XR设备获取）
        dual_gripper_data_lock: 数据同步锁
        dual_gripper_state_out: [output] 返回左(1)、右(1)夹爪电机状态
        dual_gripper_action_out: [output] 返回左(1)、右(1)夹爪电机动作
        fps: 控制频率
        Unit_Test: 是否启用单元测试
        """
    
        
        self.fps = fps
        self.Unit_Test = Unit_Test
        
        # 初始化模拟的夹爪状态
        self.dual_gripper_state = [0.0, 0.0]  # [右夹爪, 左夹爪]
        
        # 初始化控制线程
        self.gripper_control_thread = threading.Thread(target=self.control_thread, 
                                                     args=(left_hand_array, right_hand_array, self.dual_gripper_state,
                                                           dual_gripper_data_lock, dual_gripper_state_out, dual_gripper_action_out))
        self.gripper_control_thread.daemon = True
        self.gripper_control_thread.start()
        
        print("Initialize Gripper_Controller OK!\n")
    
    def _update_gripper_state(self, target_positions):
        """更新模拟的夹爪状态"""
        # 逐渐将当前状态向目标状态靠近，模拟夹爪运动
        for idx in range(len(self.dual_gripper_state)):
            delta = target_positions[idx] - self.dual_gripper_state[idx]
            self.dual_gripper_state[idx] += delta * 0.1  # 每次更新10%的差距
    
    def ctrl_dual_gripper(self, gripper_q_target):
        """设置当前左右夹爪电机状态目标位置"""
        # 在模拟版本中，我们直接更新夹爪状态
        self._update_gripper_state(gripper_q_target)
    
    def control_thread(self, left_hand_array, right_hand_array, dual_gripper_state_in, dual_hand_data_lock=None, 
                      dual_gripper_state_out=None, dual_gripper_action_out=None):
        """夹爪控制线程"""
        self.running = True
        
        # 夹爪参数设置
        THUMB_INDEX_DISTANCE_MIN = 0.05  # 假设拇指和食指之间的最小欧几里得距离为5厘米
        THUMB_INDEX_DISTANCE_MAX = 0.07  # 假设拇指和食指之间的最大欧几里得距离为7厘米
        LEFT_MAPPED_MIN = 0.0   # 夹爪关闭时的最小初始电机位置
        RIGHT_MAPPED_MIN = 0.0  # 夹爪关闭时的最小初始电机位置
        LEFT_MAPPED_MAX = 5.40  # 夹爪打开时的最大电机位置
        RIGHT_MAPPED_MAX = 5.40 # 夹爪打开时的最大电机位置
        
        # 初始目标动作
        left_target_action = (LEFT_MAPPED_MAX - LEFT_MAPPED_MIN) / 2.0
        right_target_action = (RIGHT_MAPPED_MAX - RIGHT_MAPPED_MIN) / 2.0
        
        # 夹爪控制循环
        try:
            while self.running:
                start_time = time.time()
                
                # 模拟夹爪动作（简化版）
                # 在实际应用中，这里会根据手部姿态计算夹爪开合度
                # 在模拟版本中，我们使用简单的周期性变化
                current_time = time.time()
                period = 5.0  # 5秒周期
                import math
                left_factor = (math.sin(2 * math.pi * current_time / period) + 1) / 2
                right_factor = (math.sin(2 * math.pi * (current_time + period/2) / period) + 1) / 2
                
                left_target_action = LEFT_MAPPED_MIN + (LEFT_MAPPED_MAX - LEFT_MAPPED_MIN) * left_factor
                right_target_action = RIGHT_MAPPED_MIN + (RIGHT_MAPPED_MAX - RIGHT_MAPPED_MIN) * right_factor
                
                # 获取当前夹爪状态
                dual_gripper_state = np.array(dual_gripper_state_in[:])
                
                # 限制夹爪动作以避免溢出
                DELTA_GRIPPER_CMD = 0.18  # 每次最大变化量
                left_actual_action = np.clip(left_target_action, dual_gripper_state[1] - DELTA_GRIPPER_CMD, dual_gripper_state[1] + DELTA_GRIPPER_CMD)
                right_actual_action = np.clip(right_target_action, dual_gripper_state[0] - DELTA_GRIPPER_CMD, dual_gripper_state[0] + DELTA_GRIPPER_CMD)
                
                dual_gripper_action = np.array([right_actual_action, left_actual_action])
                
                # 更新输出数据
                if dual_gripper_state_out is not None and dual_gripper_action_out is not None:
                    with dual_hand_data_lock:
                        dual_gripper_state_out[:] = dual_gripper_state - np.array([RIGHT_MAPPED_MIN, LEFT_MAPPED_MIN])
                        dual_gripper_action_out[:] = dual_gripper_action - np.array([RIGHT_MAPPED_MIN, LEFT_MAPPED_MIN])
                
                # 控制夹爪
                self.ctrl_dual_gripper(dual_gripper_action)
                
                # 控制循环频率
                current_time = time.time()
                time_elapsed = current_time - start_time
                sleep_time = max(0, (1 / self.fps) - time_elapsed)
                time.sleep(sleep_time)
        finally:
            print("Gripper_Controller has been closed.")


# 测试FangnuoCar双臂控制的逻辑流程
def test_fangnuo_car(use_real_arms=False):
    print("\n开始测试FangnuoCar双臂控制...")
    print(f"模式: {'真实机械臂' if use_real_arms else '模拟机械臂'}")
    
    # 创建FangnuoCar对象
    car = FangnuoCar(use_real_arms=use_real_arms)


    
    # # 测试1: 获取双臂状态
    print("\n测试2: 获取双臂状态")
    status = car.status()
    print(f"左臂位置: {status['left_arm']['position']}")
    print(f"左臂速度: {status['left_arm']['velocity']}")
    print(f"右臂位置: {status['right_arm']['position']}")
    print(f"右臂速度: {status['right_arm']['velocity']}")
    
    # # 测试2: 设置双臂目标位置
    print("\n测试3: 设置双臂目标位置")
    # 创建一个简单的目标位置 - 所有关节小角度运动
    q_target = np.zeros(12)
    q_target[0] = 0.1  # 左肩关节 (对于真实机械臂使用较小的角度)
    q_target[6] = 0.1  # 右肩关节 (对于真实机械臂使用较小的角度)
    print(f"设置目标位置: {q_target}")
    car.ctrl_dual_arm(q_target)
    # if use_real_arms:
    #     print("\n测试5: 重置双臂到初始位置")
    #     car.go_home()
    #     time.sleep(5)
    # # 测试夹爪控制
    # print("\n测试4: 夹爪控制测试")
    
    # # 获取当前夹爪状态
    gripper_state = car.get_gripper_state()
    print(f"当前夹爪状态: 左臂夹爪={gripper_state['left_gripper']:.2f}mm, 右臂夹爪={gripper_state['right_gripper']:.2f}mm")
    
    # # 测试左臂夹爪控制
    # print("\n测试4.1: 左臂夹爪控制 - 完全打开")
    # car.control_left_gripper(50.0, 1000)  # 完全打开，力矩1000
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"左臂夹爪打开后状态: {gripper_state['left_gripper']:.2f}mm")
    
    # print("\n测试4.2: 左臂夹爪控制 - 半开半闭")
    # car.control_left_gripper(25.0, 1200)  # 半开半闭，力矩1200
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"左臂夹爪半开半闭后状态: {gripper_state['left_gripper']:.2f}mm")
    
    # print("\n测试4.3: 左臂夹爪控制 - 完全闭合")
    # car.control_left_gripper(0.0, 1500)  # 完全闭合，力矩1500
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"左臂夹爪闭合后状态: {gripper_state['left_gripper']:.2f}mm")
    
    # # 测试右臂夹爪控制
    # print("\n测试4.4: 右臂夹爪控制 - 完全打开")
    # car.control_right_gripper(50.0, 1000)  # 完全打开，力矩1000
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"右臂夹爪打开后状态: {gripper_state['right_gripper']:.2f}mm")
    
    # print("\n测试4.5: 右臂夹爪控制 - 半开半闭")
    # car.control_right_gripper(25.0, 1200)  # 半开半闭，力矩1200
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"右臂夹爪半开半闭后状态: {gripper_state['right_gripper']:.2f}mm")
    
    # print("\n测试4.6: 右臂夹爪控制 - 完全闭合")
    # car.control_right_gripper(0.0, 1500)  # 完全闭合，力矩1500
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"右臂夹爪闭合后状态: {gripper_state['right_gripper']:.2f}mm")
    
    # # 测试同时控制左右夹爪
    # print("\n测试4.7: 同时控制左右夹爪 - 完全打开")
    # car.control_dual_grippers(60.0, 60.0, 1000)  # 左右夹爪完全打开
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"双夹爪打开后状态: 左={gripper_state['left_gripper']:.2f}mm, 右={gripper_state['right_gripper']:.2f}mm")
    
    # print("\n测试4.8: 同时控制左右夹爪 - 完全闭合")
    # car.control_dual_grippers(0.0, 0.0, 1500)  # 左右夹爪完全闭合
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"双夹爪闭合后状态: 左={gripper_state['left_gripper']:.2f}mm, 右={gripper_state['right_gripper']:.2f}mm")
    
    # # 测试完成后，将夹爪返回到中间位置
    # print("\n测试4.9: 将夹爪返回到中间位置")
    # car.control_dual_grippers(25.0, 25.0, 1000)  # 左右夹爪返回到中间位置
    # time.sleep(2)  # 等待夹爪动作完成
    
    # gripper_state = car.get_gripper_state()
    # print(f"夹爪最终状态: 左={gripper_state['left_gripper']:.2f}mm, 右={gripper_state['right_gripper']:.2f}mm")
    

    
    print("\n夹爪控制测试完成!")


# 如果直接运行此脚本，则执行测试
if __name__ == "__main__":
    import argparse
    
    # 创建命令行参数解析器
    parser = argparse.ArgumentParser(description="测试FangnuoCar双臂控制")
    parser.add_argument('--real', action='store_true', help="使用真实机械臂进行测试")
    args = parser.parse_args()
    
    # 根据命令行参数决定是否使用真实机械臂
    test_fangnuo_car(use_real_arms=args.real)
